/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : pipo.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <avr/eeprom.h>

#include "pipo.h"
#include "sensors.h"
#include "i2cmaster.h"
#include "imu.h"
#include "mixer.h"
#include "pwm.h"
#include "uart.h"
//#include "eepromData.h"

//#define _SIMULATOR

/*************************************************************************
 Global Variables
*************************************************************************/

volatile uint16_t g25msec; // increments every 25ms

volatile uint8_t flashRedLed;	// set this = the # of flashes @ 5fps you want from teh red led
volatile uint8_t flashGreenLed;	// set this = the # of flashes @ 5fps you want from teh green led

pipoData_t EEMEM EE_pipoData;
pipoData_t pipoData; //!!!! should this be volatile?? or declared somewhere else???

float	 deltaT;
static uint16_t lastTCNT1;
uint16_t deltaT_int;

float	attitude[3];  // Yaw,Pitch,Roll
uint8_t magnetometer = 0;

float   lp_alpha = 1;

/*************************************************************************
 delay
*************************************************************************/
void delay(uint16_t msec)
{
    uint16_t tgt = g25msec + msec/25;
    while(tgt != g25msec);
}


/*************************************************************************
 25msec interrupt
*************************************************************************/
ISR(TIMER1_COMPB_vect, ISR_NOBLOCK)  // fires on TCNT1 == OCR1B - 25ms counter
{
    g25msec++;

    if(!(g25msec & 0x07)) // every 8 x 25ms (200ms)
    {
        if(flashGreenLed)
        {
            if(LED_GREEN_IS_ON) flashGreenLed--;
            LED_GREEN_FLIP;
        }

        if(flashRedLed)
        {
            if(LED_RED_IS_ON) flashRedLed--;
            LED_RED_FLIP;
        }
    }

    OCR1B += G25MSEC_SPAN; // next 25ms counter.
}



/*************************************************************************
 main
*************************************************************************/
int main(void)
{
	// disable watchdog after the soft reset
	MCUSR = 0;
    wdt_disable();
	
	//init pins
    DDRB  = PORT_B_SETUP;    PORTB = PORT_B_INIT;
    DDRC  = PORT_C_SETUP;    PORTC = PORT_C_INIT;
    DDRD  = PORT_D_SETUP;    PORTD = PORT_D_INIT;

    //set up input pin change interrupt for inputs
    PCMSK0 |= PCMSK0_BITS;
    PCMSK1 |= PCMSK1_BITS;
    PCMSK2 |= PCMSK2_BITS;

    PCICR  |= PCICR_BITS; //enable pin change interrupts (globals are disabled at the moment)

    //set up timers
    TCCR1A = 0;				//set up 16bit timer,
    TCCR1B = (1<<CS11);  	// 8 prescaler - gives us 16,000/8 => 2000 counts pes msec

	LED_RED_OFF; LED_GREEN_ON; // Green on = device initiating
	
	uart_init(57600);	//Initialise the UART

    i2c_init();    // initialize I2C library

    pwm_init();	// initialise the pwm outputs
   
	OCR1B = TCNT1 + 10;
   
	TIMSK1 |= (1<<OCIE1B); // enable 25ms counter interrupt 

	sei();

#ifndef _SIMULATOR
	
	delay(1000); // allow rx to come online - green LED only ON
		// FlySky RX takes ~350ms to start up
	
	
	/*
	 * If the throttle is above minimum, the board enters ESC programming mode 
	 * the motor outputs follow the the throttle stick directly to allow
	 * ESC calibration and programming
	 */
	if (pwmInRaw(3)>(STICK_HI)) { // if throttle not near zero
		while (pwmInRaw(3)>(STICK_LO)) {
			LED_GREEN_OFF;LED_RED_ON;
			uint16_t t=g25msec;
			pwm_out_set(8,pwmInRaw(3));	// output(s) follow throttle input
			pwm_out_set(7,pwmInRaw(3));	
			pwm_out_set(6,pwmInRaw(3));	
			pwm_out_set(5,pwmInRaw(3));	
			setupPWMs();					// trigger a PWM frame
			while (t == g25msec);			// every 25ms
		}
		LED_GREEN_ON; LED_RED_OFF;
		for(uint8_t i=0; i<20; i++) { // 500ms of min
			uint16_t t=g25msec;
			pwm_out_set(8,-RESX);	// output(s) at minimum
			pwm_out_set(7,-RESX);	
			pwm_out_set(6,-RESX);	
			pwm_out_set(5,-RESX);	
			setupPWMs();			// trigger a pwm frame
			while (t == g25msec);	// send a frame every 25ms
		}
	}

	// set the 'zero' point for the RPY stick inputs
	calibrateSticks();// LEDS alternate during this
	
	LED_GREEN_OFF;LED_RED_OFF;
	
    // set up L3G4200D
    flashGreenLed = l3g4200d_init() ? 0 : 0xFF;  // flash green led 255 times to show error
	
	// set up ADXL345
    flashRedLed = adxl_init() ? 0 : 0xFF;   // flash red led 255 times to show error
	    
    //init HMC5883L
    magnetometer = HMC5883L_init();
	
	delay(250);
	
    if(!flashRedLed && !flashGreenLed)  // if no error in accelerometer/gyro input
    {
        calibrateGyro();
        if(magnetometer)
            flashGreenLed = 2;
        else
            flashRedLed = 5;
    }
#endif // _SIMULATOR

    LED_RED_OFF; LED_GREEN_OFF; // leds off = ready to run!

    ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
    {
        lastTCNT1 = TCNT1;
    }

    //main loop
    while (1)
    {
		do {		//calculate real deltaT between measurements
			cli();	deltaT_int = TCNT1;	sei();
			deltaT_int -= lastTCNT1;	// deltaT_int = curr_time - last_time
		} while(deltaT_int < 5000);		// Restrict main Loop max 200hz for testing
		
		lastTCNT1 += deltaT_int;  // last_time = last_time + deltaT_int
        deltaT = (float)deltaT_int/TICKS_PER_SEC;  // deltaT [sec]

        lp_alpha = deltaT * (VAR_RC + deltaT);  // calculate lp alpha for LP filter

#ifndef _SIMULATOR
        //READ sensor data values
        populateSensorData();
#endif // _SIMULATOR

        //IMU Logic  -  using a 5msec loop dt
        if(magnetometer)
			AHRS();// calculate DCM and populate attitude var
		else
			IMUupdate();
		getAttitude();
	    
        //run mixer
        mixer();
		setupPWMs();

    }
}
